#include "sensor_config.h"
#include "sensor_electrode.h"
#include "sensor_celectrode.h"
#include "sensor_welectrode.h"
#include "sensor_kalman.h"
#include "sensor_glucose.h"


// Constant definition

#define BUFFER_SIZE                 8
#define WE_LOW_COUNT                4
#define WE_HYPOSENS_COUNT        ( 12 * SAMPLES_PER_HOUR )

// Private variable definition

#ifdef ENABLE_DIFF_DETECTION
#define USE_DEFAULT_REF
#define REF_LOW_COUNT_MAX           6
static fixed32 m_f32_defaultIsigRef = 0;
static fixed32 m_f32_isigRef = 0;
static uint m_ui_stableRefCntr = 0;
static uint8 m_u8_isigRefLowCntr = 0;
#endif
static uint m_ui_refValidCntr = 0;
static uint8 m_u8_refStable = 0;

static fixed32 m_f32_current[COUNT_DATAS] = {0};
static fixed32 m_f32_bufferData[COUNT_BUFFERS][BUFFER_SIZE] = {0};
static cycle_buffer m_t_buffer[COUNT_BUFFERS] = {0};

#define ISIG_DAY_BUFFER_SIZE (SAMPLES_PER_HOUR * 20)
static fixed32 m_f32_isigDayBufferData[ISIG_DAY_BUFFER_SIZE] = {0};
static fixed32 m_f32_isigDayBufferTemp[ISIG_DAY_BUFFER_SIZE] = {0};
static cycle_buffer m_t_isigDayBuffer = {0};

static fixed32 m_f32_isigP5 = 0;
static fixed32 m_f32_isigP25 = 0;
static fixed32 m_f32_isigP95 = 0;

static uint32 m_u32_isigLowCount = 0;
static uint32 m_u32_hyposensCount = 0;
static uint32 m_u32_snrCount = 0;
static uint32 m_u32_snrCountLowSNR = 0;
static fixed32 m_f32_snrIsig = 0;
static int32 m_i32_snrNoise = 0;

static fixed32 m_f32_isigBeforeDropout = 0;
static uint m_ui_dropoutCntr = 0;
static uint m_ui_dropoutStableCntr = 0;
static fixed32 m_f32_dropoutExitIsig = 0;
static kalman_filter m_t_isigKFilter;

static struct
{
    logical b_dorpoutRef;
    logical b_overshootRef;
    logical b_dropout;
    logical b_overshoot;
    logical b_noiseBlank;
    logical b_noiseTerminate;
} m_t_flag = {L_FALSE};


// Private function declaration

static void FirFilterIsig(void );
#ifdef ENABLE_VCOUNTER_ADJUSTMENT
static fixed32 VCounterAdjust(fixed32 f32_isig);
#endif
#ifdef ENABLE_NONLINEAR_ADJUSTMENT
static fixed32 NonlinearAdjust(fixed32 f32_isig);
#endif
static void CheckRawIsig(void);
static void CalculateNoise(void);
static void CheckIsig(void);
static void CheckSNR(void);
static void CheckDropout(void);
static void KalmanFilterIsig(void);
static uint32 CnscSamples( logical b_bad );


// Function definition

void WE_Initialize(void)
{
    uint32 i;

    for ( i=0; i<COUNT_DATAS; i++ )
        m_f32_current[i] = 0;
    m_f32_current[ISIG_WRK] = DP.ISIG_REF_DEFAULT;
    m_f32_current[ISIG_REF] = DP.ISIG_REF_DEFAULT;

#ifdef ENABLE_DIFF_DETECTION
    m_f32_defaultIsigRef = DP.ISIG_REF_DEFAULT;
    m_f32_isigRef = DP.ISIG_REF_DEFAULT;
    m_ui_stableRefCntr = 0;
#endif

    for ( i=0; i<COUNT_BUFFERS; i++ )
        Buffer_Init( &m_t_buffer[i], &m_f32_bufferData[i][0], BUFFER_SIZE );

    Buffer_Init(&m_t_isigDayBuffer, m_f32_isigDayBufferData, ISIG_DAY_BUFFER_SIZE);
    m_f32_isigP5 = -1;
    m_f32_isigP25 = -1;
    m_f32_isigP95 = -1;

    m_ui_refValidCntr = 0;
    m_u8_refStable = 0;

    m_u32_isigLowCount = 0;
    m_u32_hyposensCount = 0;

    m_u32_snrCount = 0;
    m_u32_snrCountLowSNR = 0;
    m_f32_snrIsig = 0;
    m_i32_snrNoise = 0;

    m_ui_dropoutCntr = 0;
    m_ui_dropoutStableCntr = 0;
    m_f32_dropoutExitIsig = 0;

    m_t_flag.b_dorpoutRef = L_FALSE;
    m_t_flag.b_overshootRef = L_FALSE;
    m_t_flag.b_dropout = L_FALSE;
    m_t_flag.b_overshoot = L_FALSE;
    m_t_flag.b_noiseBlank = L_FALSE;
    m_t_flag.b_noiseTerminate = L_FALSE;

    KF_Initialize( &m_t_isigKFilter );
}


void WE_UpdateIsig( uint32 u32_time )
{
    m_f32_current[ISIG_TIME] = u32_time;
    FirFilterIsig();
    CheckRawIsig();
    CalculateNoise();
    CheckIsig();
    if ( u32_time < INIT_SECONDS/2 )
    {
        KF_Initialize( &m_t_isigKFilter );
    }
    else
    {
        KalmanFilterIsig();
    }
}

static void FirFilterIsig(void)
{
    static int32 i32_firCoef[] = FIR_FILTER_COEF;
    static filter_window t_firWin = { i32_firCoef,
                                      FIR_FILTER_LENGTH,
                                      0 };

#ifdef FIR_FILTER_COEF2
    static int32 i32_firCoef2[] = FIR_FILTER_COEF2;
    static filter_window t_firWin2 = { i32_firCoef2,
                                      FIR_FILTER_LENGTH,
                                      0 };
#endif

    fixed32 f32_preIsigWrk = m_f32_current[ISIG_WRK];
    fixed32 f32_preIsigRef = m_f32_current[ISIG_REF];

    m_f32_current[ISIG_WRK] = Buffer_TrimedFilter( Electrode_GetFirBuffer(ELECTRODE_W1), &t_firWin,
                                                   410 * PERCISION, 0 );
    m_f32_current[ISIG_REF] = Buffer_TrimedFilter( Electrode_GetFirBuffer(ELECTRODE_W2), &t_firWin,
                                                   410 * PERCISION, 0 );
    fixed32 f32_var1 = Buffer_TrimedVar( Electrode_GetFirBuffer(ELECTRODE_W1), 7);
    fixed32 f32_var2 = Buffer_MeanAcc( Electrode_GetFirBuffer(ELECTRODE_W1), 7) / 2;
    m_f32_current[ISIG_VAR] = F_MIN(f32_var1, f32_var2);
    Buffer_Push(&m_t_buffer[BUF_ISIG_VAR], m_f32_current[ISIG_VAR]);
    if (m_f32_current[ISIG_WRK] < 5*PERCISION)
    {
        if (m_u32_isigLowCount < WE_LOW_COUNT)
            m_u32_isigLowCount++;
    }
    else
    {
        m_u32_isigLowCount = 0;
    }

    if (m_f32_current[ISIG_WRK] < 8*PERCISION)
    {
        if (m_u32_hyposensCount < WE_HYPOSENS_COUNT)
            m_u32_hyposensCount++;
    }
    else
    {
        m_u32_hyposensCount = 0;
    }

//    m_f32_current[ISIG_WRK] = WeightMean( f32_preIsigWrk, m_f32_current[ISIG_WRK], F_MIN(m_f32_current[ISIG_VAR]/10, PERCISION) );
#ifdef FIR_FILTER_COEF2
    m_f32_current[ISIG_REF] = Buffer_TrimedFilter( Electrode_GetFirBuffer(ELECTRODE_W2), &t_firWin2,
                                                   410 * PERCISION, 0 );
#endif
    m_f32_current[ISIG_WRK] = F_MAX(m_f32_current[ISIG_WRK], 0);
    m_f32_current[ISIG_REF] = F_MAX(m_f32_current[ISIG_REF], 0);

    if (m_f32_current[ISIG_WRK] > 0)
        Buffer_Push(&m_t_isigDayBuffer, m_f32_current[ISIG_WRK]);

    if (m_t_isigDayBuffer.u32_count == m_t_isigDayBuffer.u32_length)
    {
        uint32 i;
        for(i=0; i<m_t_isigDayBuffer.u32_count; i++)
            m_f32_isigDayBufferTemp[i] = m_f32_isigDayBufferData[i];
        QuickSort(m_f32_isigDayBufferTemp, 0, m_t_isigDayBuffer.u32_count-1);

        m_f32_isigP5  = m_f32_isigDayBufferTemp[m_t_isigDayBuffer.u32_count *  5 / 100];
        m_f32_isigP25 = m_f32_isigDayBufferTemp[m_t_isigDayBuffer.u32_count * 25 / 100];
        m_f32_isigP95 = m_f32_isigDayBufferTemp[m_t_isigDayBuffer.u32_count * 98 / 100];
    }

    if (m_f32_current[ISIG_REF] < DP.ISIG_REF_DEFAULT/4)
    {
        if (m_u8_isigRefLowCntr < REF_LOW_COUNT_MAX)
            m_u8_isigRefLowCntr++;
    }
    else
    {
        m_u8_isigRefLowCntr = 0;
    }


#define REF_UPPER_START     ( 550 * PERCISION / 100 )
#define REF_UPPER_END       ( 250 * PERCISION / 100 )
#define REF_UPPER_END_TIME  (  10 * SECONDS_PER_HOUR )
#define REF_VALID_MIN       (   1 * PERCISION )
#define REF_VALID_COUNT         6

    if ( m_f32_current[ISIG_REF] > REF_VALID_MIN ||
         m_f32_current[ISIG_REF] > DP.ISIG_REF_DEFAULT * 6/10 )
    {
        if (m_ui_refValidCntr < REF_VALID_COUNT - 1)
            m_ui_refValidCntr++;
        else
            m_t_flag.b_dorpoutRef = L_FALSE;
    }
    else
    {
        m_ui_refValidCntr = 0;
        m_t_flag.b_dorpoutRef = L_TRUE;
    }

    m_f32_current[ROC_WRK] = m_f32_current[ISIG_WRK] - f32_preIsigWrk;
    m_f32_current[ROC_REF] = m_f32_current[ISIG_REF] - f32_preIsigRef;
    Buffer_Push( &m_t_buffer[BUF_ROC_REF], m_f32_current[ROC_REF] );

#ifdef ENABLE_DIFF_DETECTION
#define MIN_ISIG_REF_ROC       ( 10 * PERCISION / 100 )
#define MAX_ISIG_REF_ROC       ( 40 * PERCISION / 100 )
#define STABLE_REF_COUNT          6

    fixed32 f32_minIsigRef;
    f32_minIsigRef = F_MAX(DP.ISIG_REF_DEFAULT/4, m_f32_defaultIsigRef/2);
    f32_minIsigRef = F_MAX(f32_minIsigRef, 50*PERCISION/100);
    fixed32 f32_minIsigRefRoc = F_MAX( DP.ISIG_REF_DEFAULT, 200 * PERCISION / 100 );
    f32_minIsigRefRoc = F_MUL( f32_minIsigRefRoc, MIN_ISIG_REF_ROC );
    fixed32 f32_maxIsigRefRoc = F_MAX( DP.ISIG_REF_DEFAULT, 200 * PERCISION / 100 );
    f32_maxIsigRefRoc = F_MUL( f32_maxIsigRefRoc, MAX_ISIG_REF_ROC );

#ifdef ENABLE_OVERSHOOT_CORRECTION
    if (m_t_flag.b_overshootRef)
    {
        m_t_flag.b_overshootRef = m_f32_current[ISIG_REF] > m_f32_defaultIsigRef &&
                                ( ABS(m_f32_current[ROC_REF]) > f32_maxIsigRefRoc
                               || Buffer_AbsMean( &m_t_buffer[BUF_ROC_REF], 5 ) > f32_maxIsigRefRoc );
    }
    else
    {
        m_t_flag.b_overshootRef = ( m_f32_current[ROC_REF] > 0
                                 && f32_preIsigRef < m_f32_defaultIsigRef * 3/2
                                 && m_f32_current[ROC_REF]-m_f32_current[ROC_WRK] > m_f32_defaultIsigRef *3/2 ) ||
                                  ( m_f32_current[ISIG_REF] > m_f32_defaultIsigRef
                                 && f32_preIsigRef < f32_minIsigRef
                                 && m_f32_current[ROC_REF]-m_f32_current[ROC_WRK] > f32_maxIsigRefRoc );
    }
#endif

    fixed32 f32_rocRation = ABS(m_f32_current[ROC_REF]) < f32_minIsigRefRoc
                          ? 0
                          : ( m_f32_current[ROC_WRK] == 0
                            ? F_DIV(m_f32_current[ROC_REF], 1)
                            : F_DIV(m_f32_current[ROC_REF], m_f32_current[ROC_WRK])
                            ) ;

    if ( m_t_flag.b_overshootRef ||
         m_f32_current[ISIG_REF] < f32_minIsigRef ||
         f32_rocRation > 15*PERCISION/10 ||
         f32_rocRation < -2*PERCISION/10 )
    {
        m_ui_stableRefCntr = 0;
    }
    else if ( m_ui_stableRefCntr <= STABLE_REF_COUNT )
    {
        m_ui_stableRefCntr++;
    }

    if (m_f32_current[ISIG_TIME] <= 4*SECONDS_PER_HOUR)
    {
        m_f32_isigRef = m_f32_current[ISIG_REF];
        if (m_f32_current[ISIG_TIME] > 3*SECONDS_PER_HOUR)
        {
            if (m_ui_stableRefCntr == STABLE_REF_COUNT)
                m_f32_defaultIsigRef = m_f32_current[ISIG_REF];
            else if (m_ui_stableRefCntr > STABLE_REF_COUNT)
                m_f32_defaultIsigRef = WeightMean( m_f32_defaultIsigRef, m_f32_current[ISIG_REF], 90 );
        }
    }
#ifdef USE_DEFAULT_REF
    else
    {
        m_f32_isigRef = WeightMean( m_f32_isigRef, DP.ISIG_REF_DEFAULT, 90 );
    }
#else
    else if (m_ui_stableRefCntr >= STABLE_REF_COUNT)
    {
        m_f32_isigRef = m_f32_current[ISIG_REF];
        if ( m_f32_current[ISIG_REF]  > DP.ISIG_REF_DEFAULT/4 &&
             m_f32_current[ISIG_REF]  < DP.ISIG_REF_DEFAULT*4 )
        {
            if (m_ui_stableRefCntr == STABLE_REF_COUNT)
                m_f32_defaultIsigRef = m_f32_current[ISIG_REF];
            else
                m_f32_defaultIsigRef = WeightMean( m_f32_defaultIsigRef, m_f32_current[ISIG_REF], 90 );
        }
    }
    else
    {
        m_f32_defaultIsigRef = WeightMean( m_f32_defaultIsigRef, DP.ISIG_REF_DEFAULT, 98 );
        if (m_ui_stableRefCntr > 0)
        {
            m_f32_isigRef = WeightMean( m_f32_isigRef, m_f32_current[ISIG_REF], F_DIV( STABLE_REF_COUNT-m_ui_stableRefCntr, STABLE_REF_COUNT ) );
        }
        else
        {
            m_f32_isigRef = WeightMean( m_f32_isigRef, m_f32_defaultIsigRef, 70 );
        }
    }
#endif

    m_f32_isigRef = F_MAX(m_f32_isigRef, DP.ISIG_REF_DEFAULT);
#ifndef USE_DEFAULT_REF
    if (m_u8_refStable == 0)
    {
        fixed32 f32_isigRefUpper = F_DIV(m_f32_current[ISIG_TIME], REF_UPPER_END_TIME);
        f32_isigRefUpper = F_MIN(f32_isigRefUpper, PERCISION);
        f32_isigRefUpper = F_MUL(f32_isigRefUpper, REF_UPPER_END-REF_UPPER_START);
        f32_isigRefUpper += REF_UPPER_START;
        if (WITHIN(m_f32_current[ISIG_REF], DP.ISIG_REF_DEFAULT, f32_isigRefUpper) && m_f32_isigRef < f32_isigRefUpper)
            m_u8_refStable = 1;
        else
            m_f32_isigRef = F_MIN(m_f32_isigRef, f32_isigRefUpper);
    }
#endif

    m_f32_current[ISIG_RAW] = m_f32_current[ISIG_WRK] - m_f32_isigRef;
    m_f32_current[ISIG_RAW] = F_MAX(m_f32_current[ISIG_RAW], 0);
#else
    m_f32_current[ISIG_RAW] = m_f32_current[ISIG_WRK];
#endif

#ifdef ENABLE_VCOUNTER_ADJUSTMENT
    m_f32_current[ISIG_RAW] = VCounterAdjust(m_f32_current[ISIG_RAW]);
#endif

#ifdef ENABLE_NONLINEAR_ADJUSTMENT
    m_f32_current[ISIG_RAW] = NonlinearAdjust(m_f32_current[ISIG_RAW]);
#endif

    Buffer_Push( &m_t_buffer[BUF_ISIG_RAW], m_f32_current[ISIG_RAW] );
}

#ifdef ENABLE_VCOUNTER_ADJUSTMENT
#define VCOUNTER_ADJUSTMENT_C1      ( 20 * PERCISION )
#define VCOUNTER_ADJUSTMENT_C2      (  5 * PERCISION )
static fixed32 VCounterAdjust(fixed32 f32_isig)
{ 
    if (CE_Valid() && CE_VCounterHigh())
    {
        fixed32 f32_vc = CE_GetVCounter();
        f32_vc -= VOLTAGE_REF;
        fixed32 f32_vcRoc = CE_GetVCounterRoc();
        f32_isig += F_MUL(f32_vc, VCOUNTER_ADJUSTMENT_C1) + F_MUL(f32_vcRoc, VCOUNTER_ADJUSTMENT_C2);
    }

    return f32_isig;
}
#endif

#ifdef ENABLE_NONLINEAR_ADJUSTMENT
static fixed32 NonlinearAdjust(fixed32 f32_isig)
{
    fixed32 f32_temp;
//    if (f32_isig > DP.ISIG_NONLINEAR_C0)
//    {
//        f32_temp = f32_isig - (fixed32)DP.ISIG_NONLINEAR_C0;
//        f32_temp = F_MUL( (fixed32)DP.ISIG_NONLINEAR_C1, f32_temp );
//        f32_isig += f32_temp;
//    }

    f32_temp = f32_isig - (fixed32)DP.ISIG_NONLINEAR_C0;
    f32_temp = F_MUL( (fixed32)DP.ISIG_NONLINEAR_C1, f32_temp );
    f32_temp = F_MUL( f32_isig, f32_temp ) / PERCISION;
    f32_isig += f32_temp;

    return f32_isig;
}
#endif

logical WE_Assert(void)
{
#define ISIG_MIN     ( 0*PERCISION)
#define ISIG_MAX     (60*PERCISION)
    return m_f32_current[ISIG_RAW]  > ISIG_MIN &&
           m_f32_current[ISIG_RAW]  < ISIG_MAX &&
           m_f32_current[ISIG_FILT] > ISIG_MIN &&
           !(m_t_flag.b_dropout && m_ui_dropoutCntr >= SAMPLES_PER_HOUR - 1);
}

logical WE_NoiseHigh(void)
{
    uint8 u8_noiseHighCount = 0;
    uint8 i;
    for(i = 0; i < 6 && i < m_t_buffer[BUF_ISIG_VAR].u32_count; i++)
    {
        if (Buffer_Get(&m_t_buffer[BUF_ISIG_VAR], i) > PERCISION)
            u8_noiseHighCount++;
    }
    return u8_noiseHighCount >= 3;
}

logical WE_Low(void)
{
    return m_u32_isigLowCount >= WE_LOW_COUNT;
}

logical WE_Hyposens(void)
{
    return m_u32_hyposensCount >= WE_HYPOSENS_COUNT;
}

logical WE_Nosens(void)
{
    if (m_f32_isigP5 >= 0 && m_f32_isigP25 >= 0 && m_f32_isigP95 >= 0)
    {
        fixed32 f32_d95 = m_f32_isigP95 - m_f32_isigP5;
//        fixed32 f32_d95Min = CLIP(m_f32_isigP25/10, 2*PERCISION, 1*PERCISION);
        fixed32 f32_d95Min = 5*PERCISION/10;
        if (f32_d95 < CLIP(6*PERCISION - m_f32_isigP25, 2*PERCISION, f32_d95Min))
            return L_TRUE;
        if (m_f32_isigP25 < (5*PERCISION) && f32_d95 < (25*PERCISION/10) && CE_Deficit())
            return L_TRUE;
    }
    return L_FALSE;
}

static void CheckRawIsig(void)
{
    m_f32_current[ISIG_BAD] = L_FALSE;
    if ( m_f32_current[ISIG_BAD] )
        Buffer_Push( &m_t_buffer[BUF_ISIG_RAW_NOBAD], Buffer_Get(&m_t_buffer[BUF_ISIG_RAW],1) );
    else
        Buffer_Push( &m_t_buffer[BUF_ISIG_RAW_NOBAD], m_f32_current[ISIG_RAW] );
}

static void CalculateNoise(void)
{
#define NOISE_LEVEL_WIN_SIZE             SAMPLES_PER_HOUR
#define NOISE_LEVEL_SCALE           35
#define NOISE_LEVEL_DEFAULT      ( 750 * PERCISION / 100 )
#define NOISE_LEVEL_UPPER       (10000 * PERCISION / 100 )
#define NOISE_LEVEL_LOWER        (   0 * PERCISION / 100 )
#define NOISE_LEVEL_FREQ_INVALID ( 700 * PERCISION / 100 )
#define EQUIV_FREQ_WIN_SIZE      (   2 * SAMPLES_PER_HOUR / 3 )
#define EQUIV_FREQ_SCALE             5
#define EQUIV_FREQ_DEFAULT       (  90 * PERCISION / 100 )
#define EQUIV_FREQ_UPPER         ( 400 * PERCISION / 100 )
#define EQUIV_FREQ_LOWER         (  20 * PERCISION / 100 )

    fixed32 f32_temp,
            f32_calFactor;
    uint32 u32_count;

    f32_calFactor = Glu_GetTotalCalFactor();

    m_f32_current[ROC_RAW] = ChangePerMinute( &m_t_buffer[BUF_ISIG_RAW] );
    Buffer_Push( &m_t_buffer[BUF_ROC_RAW], m_f32_current[ROC_RAW] );

    m_f32_current[ROC_SAVIT] = ChangePerMinuteFilt( &m_t_buffer[BUF_ISIG_RAW_NOBAD] );
    Buffer_Push( &m_t_buffer[BUF_ROC_SAVIT], m_f32_current[ROC_SAVIT] );

    m_f32_current[ACC_RAW] = Buffer_Diff( &m_t_buffer[BUF_ROC_RAW], 0 );
    Buffer_Push( &m_t_buffer[BUF_ACC_RAW], m_f32_current[ACC_RAW] );

    u32_count = m_t_buffer[BUF_ISIG_RAW].u32_count;
    u32_count = u32_count > 2 ? u32_count - 2 : 0;

    m_f32_current[NOISE_LEVEL] = NOISE_LEVEL_DEFAULT;
    if ( u32_count > 0 )
    {
        f32_temp = Buffer_AbsMean( &m_t_buffer[BUF_ACC_RAW], F_MIN(10, u32_count) );
        f32_temp = F_MAX( f32_temp, ABS(m_f32_current[ACC_RAW]) );
        f32_temp *= NOISE_LEVEL_SCALE;
        f32_temp = F_MUL( f32_temp, f32_calFactor );
        m_f32_current[NOISE_LEVEL] = CLIP( f32_temp, NOISE_LEVEL_UPPER, NOISE_LEVEL_LOWER );
    }
    Buffer_Push( &m_t_buffer[BUF_NOISE_LEVEL], m_f32_current[NOISE_LEVEL] );

    m_f32_current[EQUIV_FREQ] = EQUIV_FREQ_DEFAULT;
    if ( u32_count > 0 && m_f32_current[NOISE_LEVEL] < NOISE_LEVEL_FREQ_INVALID )
    {
        f32_temp = Buffer_Mean( &m_t_buffer[BUF_ROC_RAW], F_MIN(8, u32_count) );
        f32_temp = ABS( f32_temp );
        f32_temp *= EQUIV_FREQ_SCALE;
        f32_temp = F_MUL( f32_temp, f32_calFactor );
        m_f32_current[EQUIV_FREQ] = CLIP( f32_temp, EQUIV_FREQ_UPPER, EQUIV_FREQ_LOWER );
    }
}

static void CheckIsig(void)
{
    CheckSNR();
    if(m_f32_current[ISIG_TIME] > INIT_SECONDS)
    {
        CheckDropout();
    }
#ifdef ENABLE_DROPOUT_CORRECTION
    m_f32_current[ISIG_BAD] = m_t_flag.b_dropout;
#endif
    Buffer_Push( &m_t_buffer[BUF_ISIG_BAD], m_f32_current[ISIG_BAD] );
}

static void CheckSNR(void)
{
#define SNR_COUNT_TO_CHECK       (  2 * SAMPLES_PER_HOUR )
#define SNR_NOISE_HIGH              8
#define SNR_COUNT_TO_BLANK          2
#define SNR_COUNT_TO_TERMINATE      3
#define SNR_LOW_THRESHOLD        ( 50 * PERCISION )

    m_u32_snrCount++;
    m_f32_snrIsig += m_f32_current[ISIG_RAW];
    if ( m_f32_current[NOISE_LEVEL] > SNR_NOISE_HIGH )
        m_i32_snrNoise++;

    if ( m_u32_snrCount > SNR_COUNT_TO_CHECK )
    {
        if ( m_f32_snrIsig/(m_i32_snrNoise+1) < SNR_LOW_THRESHOLD )
            m_u32_snrCountLowSNR++;
        else
            m_u32_snrCountLowSNR = 0;
        m_u32_snrCount = 0;
        m_f32_snrIsig = 0;
        m_i32_snrNoise = 0;
    }

    m_t_flag.b_noiseBlank = ( m_u32_snrCountLowSNR >= SNR_COUNT_TO_BLANK );
    m_t_flag.b_noiseTerminate = ( m_u32_snrCountLowSNR >= SNR_COUNT_TO_TERMINATE );
}

static void CheckDropout(void)
{
#define OVERSHOOT_ROC                 (100 * PERCISION / 100 )
#define DROPOUT_ROC_RAPID_EX          ( 40 * PERCISION / 100 )
#define DROPOUT_ROC_RAPID             ( 30 * PERCISION / 100 )
#define DROPOUT_ROC_FAST              ( 15 * PERCISION / 100 )
#define DROPOUT_ROC_MEDIUM            ( 12 * PERCISION / 100 )
#define DROPOUT_ROC_MODERATE          ( 10 * PERCISION / 100 )
#define DROPOUT_ROC_STABLE            (  4 * PERCISION / 100 )
#define DROPOUT_ROC_SLOW              (  3 * PERCISION / 100 )
#define DROPOUT_ACC_RAPID             ( 40 * PERCISION / 100 )
#define DROPOUT_ACC_FAST              ( 18 * PERCISION / 100 )
#define DROPOUT_ACC_MODERATE          ( 14 * PERCISION / 100 )
#define DROPOUT_NOISE_LOW             (200 * PERCISION / 100 )
#define DROPOUT_NOISE_LOW_EX          (100 * PERCISION / 100 )
#define DROPOUT_EXIT_ISIG_INIT_RATIO  ( 70 * PERCISION / 100 )
#define DROPOUT_EXIT_ISIG_CHANGE_RATE ( 50 * PERCISION / 100 )
#define DROPOUT_COUNT_MAX               12
#define DROPOUT_COUNT_MAX_FIRST         24
#define DROPOUT_STABLE_COUNT_MAX         3
//#define DROPOUT_FIRST_SECONDS           48 * SECONDS_PER_HOUR

    logical b_temp = L_FALSE;
    fixed32 f32_roc;
    uint8 u8_countMax;
    fixed32 f32_temp;
    fixed32 f32_calFactor = Glu_GetTotalCalFactor();

    f32_roc = F_MUL( m_f32_current[ROC_RAW], f32_calFactor );

#ifdef DROPOUT_FIRST_SECONDS
    if ( m_f32_current[ISIG_TIME] < DROPOUT_FIRST_SECONDS )
    {
        u8_countMax = DROPOUT_COUNT_MAX_FIRST;
    }
    else
#endif
    {
        u8_countMax = DROPOUT_COUNT_MAX;
    }

    if (m_t_flag.b_overshoot)
    {
        m_ui_dropoutCntr++;
        if (m_ui_dropoutCntr < u8_countMax/2)
            m_f32_dropoutExitIsig -= F_DIV( DROPOUT_EXIT_ISIG_CHANGE_RATE, f32_calFactor );
        if (m_ui_dropoutCntr > u8_countMax)
            m_t_flag.b_overshoot = L_FALSE;
        else if (m_ui_dropoutStableCntr > 0)
        {
            if (ABS(f32_roc) < DROPOUT_ROC_MODERATE)
                m_ui_dropoutStableCntr++;
            if (ABS(f32_roc) < DROPOUT_ROC_SLOW || m_ui_dropoutStableCntr > DROPOUT_STABLE_COUNT_MAX)
                m_t_flag.b_overshoot = L_FALSE;
        }
        else if (f32_roc > -DROPOUT_ROC_MEDIUM || m_f32_current[ISIG_RAW] <= m_f32_dropoutExitIsig)
        {
            m_ui_dropoutStableCntr = 1;
        }
        if (m_t_flag.b_overshoot)
            return;
    }

    if (m_t_flag.b_dropout)
    {
        m_ui_dropoutCntr++;
        if (m_ui_dropoutCntr < u8_countMax/2)
            m_f32_dropoutExitIsig -= DROPOUT_EXIT_ISIG_CHANGE_RATE;
        if (m_ui_dropoutCntr > u8_countMax)
            m_t_flag.b_dropout = L_FALSE;
        else if (m_ui_dropoutStableCntr > 0)
        {
            if (f32_roc < -DROPOUT_ROC_MEDIUM)
            {
                m_ui_dropoutStableCntr = 0;
                return;
            }
            if (ABS(f32_roc) < DROPOUT_ROC_MODERATE)
                m_ui_dropoutStableCntr++;
            if (ABS(f32_roc) < DROPOUT_ROC_SLOW || m_ui_dropoutStableCntr > DROPOUT_STABLE_COUNT_MAX)
                m_t_flag.b_dropout = L_FALSE;
        }
        else if (f32_roc > DROPOUT_ROC_MEDIUM || m_f32_current[ISIG_RAW] >= m_f32_dropoutExitIsig)
        {
            m_ui_dropoutStableCntr = 1;
        }
        if (m_t_flag.b_dropout)
            return;
    }

    if ( Glu_GetState() != GLUCOSE_ERROR && m_t_buffer[BUF_ISIG_RAW].u32_count >= 5 )
    {
        if (m_f32_current[ISIG_TIME] > 2*SECONDS_PER_HOUR)
            m_t_flag.b_overshoot = f32_roc > OVERSHOOT_ROC;

#ifdef DROPOUT_FIRST_SECONDS
        if (!b_temp)
        {
            if (m_f32_current[ISIG_TIME] < DROPOUT_FIRST_SECONDS)
            {
                f32_temp = Buffer_Get(&m_t_buffer[BUF_ROC_RAW], 1);
                f32_temp = F_MUL( f32_temp, f32_calFactor );
                b_temp = f32_temp < -DROPOUT_ROC_STABLE;

                f32_temp = Buffer_Get(&m_t_buffer[BUF_ROC_RAW], 2);
                f32_temp = F_MUL( f32_temp, f32_calFactor );
                b_temp = b_temp && f32_temp < -DROPOUT_ROC_STABLE;

                fixed32 f32_sg1 = Buffer_Get(&m_t_buffer[BUF_ISIG_RAW], 1);
                fixed32 f32_sg2 = Buffer_Get(&m_t_buffer[BUF_ISIG_RAW], 2);
                f32_sg1 = F_MIN(f32_sg1, f32_sg2);
                f32_sg1 = F_MUL( f32_sg1, f32_calFactor );
                b_temp = (!b_temp) && (f32_roc < -F_MAX(f32_sg1/40, DROPOUT_ROC_FAST));
            }
        }
#endif

        if (!b_temp)
        {
            b_temp = f32_roc < -DROPOUT_ROC_RAPID_EX;
        }
        if (!b_temp)
        {
            b_temp = f32_roc < -DROPOUT_ROC_RAPID;

            f32_temp = Buffer_Get(&m_t_buffer[BUF_ACC_RAW], 1);
            f32_temp = F_MUL( f32_temp, f32_calFactor );
            b_temp = b_temp && ABS(f32_temp) < DROPOUT_ACC_RAPID;

            f32_temp = Buffer_Get(&m_t_buffer[BUF_ACC_RAW], 2);
            f32_temp = F_MUL( f32_temp, f32_calFactor );
            b_temp = b_temp && ABS(f32_temp) < DROPOUT_ACC_RAPID;

            f32_temp = Buffer_Get(&m_t_buffer[BUF_ROC_RAW], 1);
            f32_temp = F_MUL( f32_temp, f32_calFactor );
            b_temp = b_temp && ABS(f32_temp) < DROPOUT_ROC_STABLE;
        }
        if (!b_temp)
        {
            b_temp = f32_roc < -DROPOUT_ROC_FAST
                  && F_MUL( m_f32_current[ACC_RAW], f32_calFactor ) < -DROPOUT_ACC_FAST
                  && Buffer_Get(&m_t_buffer[BUF_ROC_SAVIT], 1) > 0
                  && Buffer_Get(&m_t_buffer[BUF_NOISE_LEVEL], 1) < DROPOUT_NOISE_LOW;
        }
        if (!b_temp)
        {
            b_temp = f32_roc < -DROPOUT_ROC_MODERATE
                  && F_MUL( m_f32_current[ACC_RAW], f32_calFactor ) < -DROPOUT_ACC_MODERATE
                  && Buffer_Get(&m_t_buffer[BUF_ROC_SAVIT], 1) > -DROPOUT_ROC_SLOW
                  && Buffer_Get(&m_t_buffer[BUF_NOISE_LEVEL], 1) < DROPOUT_NOISE_LOW_EX;
        }

        if (b_temp)
        {
            m_t_flag.b_dropout = L_TRUE;
        }
    }

    if (m_t_flag.b_overshoot || m_t_flag.b_dropout)
    {
        m_f32_isigBeforeDropout = Buffer_Get(&m_t_buffer[BUF_ISIG_RAW], 1);
        f32_temp = m_f32_isigBeforeDropout - m_f32_current[ISIG_RAW];
        m_f32_dropoutExitIsig = m_f32_current[ISIG_RAW] + F_MUL( f32_temp, DROPOUT_EXIT_ISIG_INIT_RATIO );

        m_ui_dropoutCntr = 0;
        m_ui_dropoutStableCntr = 0;
    }
}


static void KalmanFilterIsig(void)
{
#define COMP_PRED_MINUTES            5
#define COMP_RAW_ROC_WEIGHT      (  30 * PERCISION / 100 )
#define COMP_NOISE_UPPER         ( 300 * PERCISION / 100 )
#define COMP_NOISE_LOWER         ( 100 * PERCISION / 100 )
#define COMP_GLU_ROC_UPPER       (  20 * PERCISION / 100 )
#define COMP_MAX_SHIFT           (  12 * PERCISION )
#define COMP_MAX_SHIFT_FIRST     (  24 * PERCISION )
#define COMP_BAD_RAW_ISIG_WEIGHT_LOWER (  15 * PERCISION / 100 )
#define COMP_BAD_RAW_ISIG_WEIGHT_UPPER (  85 * PERCISION / 100 )
#define COMP_BAD_RAW_ISIG_COUNT           6
#define COMP_BAD_RAW_ISIG_COUNT_FIRST     18

    fixed32 f32_isig;
    fixed32 f32_roc;
    fixed32 f32_maxShift, f32_badCount, f32_scale;
    fixed32 f32_calFactor = Glu_GetTotalCalFactor();
    fixed32 f32_rocUpper = F_DIV( COMP_GLU_ROC_UPPER, f32_calFactor );
  
    f32_roc = WeightMean( m_f32_current[ROC_RAW], m_f32_current[ROC_SAVIT], COMP_RAW_ROC_WEIGHT );
//    f32_roc = CLIP(f32_roc, f32_rocUpper, -f32_rocUpper);

    if (m_f32_current[ISIG_BAD])
    {
        f32_scale = ABS(m_f32_current[ISIG_RAW] - m_f32_isigBeforeDropout);
#ifdef DROPOUT_FIRST_SECONDS
        if ( m_f32_current[ISIG_TIME] < DROPOUT_FIRST_SECONDS )
        {
            f32_maxShift = COMP_MAX_SHIFT_FIRST;
            f32_badCount = COMP_BAD_RAW_ISIG_COUNT_FIRST;
        }
        else
#endif
        {
            f32_maxShift = COMP_MAX_SHIFT;
            f32_badCount = COMP_BAD_RAW_ISIG_COUNT;
        }
        f32_scale = f32_scale * f32_badCount / F_MIN(m_ui_dropoutCntr+1, f32_badCount);
        f32_scale = PERCISION - F_DIV( F_MIN(f32_scale, f32_maxShift), f32_maxShift ) + COMP_BAD_RAW_ISIG_WEIGHT_LOWER;
        f32_scale = F_MIN(f32_scale, COMP_BAD_RAW_ISIG_WEIGHT_UPPER) * F_MIN(m_ui_dropoutCntr, f32_badCount) / f32_badCount;
        f32_isig = WeightMean( m_f32_current[ISIG_RAW], m_f32_current[ISIG_FILT], f32_scale );
        f32_roc = CLIP(f32_roc, f32_rocUpper, -f32_rocUpper);
        f32_roc = F_MUL( f32_roc, f32_scale );
    }
    else
    {
        if (m_f32_current[ISIG_FILT] > 0)
        {
            f32_scale = Sqrt(m_f32_current[ISIG_VAR] * 100) * 2 / 5;
            f32_scale = CLIP(f32_scale, 95, 0);
            f32_isig = WeightMean( m_f32_current[ISIG_FILT], m_f32_current[ISIG_RAW], f32_scale );
            f32_scale = Buffer_Count(&m_t_buffer[BUF_ISIG_BAD], BUFFER_SIZE) * PERCISION / BUFFER_SIZE;
            f32_isig = WeightMean( m_f32_current[ISIG_FILT], f32_isig, f32_scale );
        }
        else
        {
            f32_scale = PERCISION;
            f32_isig = m_f32_current[ISIG_RAW];
        }
        if ( f32_scale > 0 )
        {
            f32_roc = 0;
        }
        else
        {
            f32_scale = PERCISION - CLIP_NORM( m_f32_current[NOISE_LEVEL], COMP_NOISE_UPPER, COMP_NOISE_LOWER );
            f32_roc = F_MUL( f32_roc, f32_scale );
            f32_roc = CLIP(f32_roc, f32_rocUpper, -f32_rocUpper);  
        }
        f32_isig = f32_isig + f32_roc * COMP_PRED_MINUTES;
    }
    KF_AdjustR( &m_t_isigKFilter, m_f32_current[NOISE_LEVEL] );
    KF_AdjustQ( &m_t_isigKFilter, m_f32_current[EQUIV_FREQ], m_f32_current[NOISE_LEVEL] );
    f32_roc = f32_roc * MINUTES_PER_HOUR/SAMPLES_PER_HOUR;
    f32_rocUpper = f32_rocUpper * MINUTES_PER_HOUR/SAMPLES_PER_HOUR / 2;
    f32_roc = CLIP(f32_roc, f32_rocUpper, -f32_rocUpper);
    m_f32_current[ISIG_FILT] = KF_Update( &m_t_isigKFilter, f32_isig, f32_roc);// + m_t_isigKFilter.f32_x[1];
    Buffer_Push( &m_t_buffer[BUF_ISIG_FILT], m_f32_current[ISIG_FILT]);
}

fixed32 WE_GetCurrent( we_data_type t_dataType )
{
    return m_f32_current[t_dataType];
}

fixed32 WE_GetCurrentIsigRef(void)
{
    return m_f32_isigRef;
}

const cycle_buffer* WE_GetBuffer( we_buffer_type t_bufferType )
{
    return &m_t_buffer[t_bufferType];
}

logical WE_IsRefValid(void)
{
    return m_u8_isigRefLowCntr < REF_LOW_COUNT_MAX;
}

uint32 WE_CnscBadSamples(void)
{
    return CnscSamples( L_TRUE );
}

uint32 WE_CnscNormalSamples(void)
{
    return CnscSamples( L_FALSE );
}

static uint32 CnscSamples( logical b_bad )
{
    uint32 i,u32_count = 0;
    cycle_buffer t_isigBad = m_t_buffer[BUF_ISIG_BAD];

    for( i=0; i<t_isigBad.u32_count; i++ )
    {
        if ( (logical)Buffer_Get(&t_isigBad, i) == b_bad )
            u32_count++;
        else
            break;
    }
    return u32_count;
}
